#define OPENCV
#include <thread>
#include <opencv2/opencv.hpp>
#include <atomic>
#include <stdlib.h>
#include <time.h>
#define printmsg(sg) std::cout << sg << std::endl
#if defined(_MSC_VER) && (_MSC_VER >= 1200)
#define dxnum 1.0
#include <Windows.h>
#else
#define dxnum 0.001
#include <unistd.h>
#define Sleep(x) usleep(x * 1000)
#endif
/*
timechip
pic handle
net handle
detect handle

*/
//using namespace udpio;
//using namespace std;
#define robotmessage "Build Time:" + string(__TIME__) + " Date:" + __DATE__ + "\nCopyRight@2019-2020\nAUV(zzu) is connected.\nStart to send messages..."

int calFPS()
{
	using namespace std;
#if defined(_MSC_VER) && (_MSC_VER >= 1200)
#define dxnum 1.0
#else
#define dxnum 0.0001
#endif
	static int dtime;
	static int cnt = 0;
	static long time = 0;
	static int temp = -1;
	cnt++;
	dtime += dxnum*(clock()-time);
	time = clock();
	if (dtime>=1000)
	{
		dtime = 0;
		temp = cnt;
		cnt = 0;
	}
	cout<<"FPS:"<<temp<<endl;
	return temp;
	
}

int main(int argc, char *argv[])
{
	using namespace std;
	using namespace cv;
	namedWindow("a");
	FileStorage fs("inCailFilePath.xml", FileStorage::READ);
	Mat intrinsics, distortion, map1, map2;

	fs["camera_matrix"] >> intrinsics;
	fs["distortion_coefficients"] >> distortion;
	fs.release();

	initUndistortRectifyMap(
		intrinsics, distortion, Mat(),
		getOptimalNewCameraMatrix(intrinsics, distortion, Size(640, 480), 1, Size(1000, 750), 0),
		Size(1000, 750),
		CV_16SC2, map1, map2);
	cv::VideoCapture cap(0);
	cv::Mat q;
	auto net = dnn::readNetFromDarknet("auv.cfg","auv.tech");
	//net.setPreferableBackend(dnn::DNN_BACKEND_CUDA);
	//net.setPreferableTarget(dnn::DNN_TARGET_CUDA);
	net.setPreferableTarget(dnn::DNN_TARGET_CPU);
	auto mod = cv::dnn::dnn4_v20210608::DetectionModel(net);
	mod.setInputSize(Size(640,480));
	mod.setInputScale(1.0 / 255);
	mod.setInputSwapRB(true);
	long lasttime1 = clock();
	int dtime1 = 0,cnt = 0;
	while (1)
	{
		
		if (cap.isOpened())
		{
			cap>>(q);
			remap(q, q, map1, map2, INTER_LINEAR);
			vector<int>ids;
			vector<float>cfds;
			vector<Rect>bxs;
			//mod.processFrame(q,blob);
			mod.detect(q,ids,cfds,bxs,0.5,0.4);
			calFPS();
			if(dtime1>60)
			{
				dtime1 = 0;
			}	
			for(auto i:bxs)
			{
				rectangle(q,i,Scalar(255,255,255),2);
			}
			imshow("a",q);
			waitKey(1);
		}
	}
	return 0;
}
